package com.htfyun.serialport;

import com.htfyun.serialport.uartservo.AX12_ServoUpdate.AX12Update_ReceivedSerialPacket;
import com.htfyun.serialport.uartservo.AX12_ServoUpdate.AX12Update_ServoReadDataAnalyze;

import java.util.List;

public class TestJava {

    private final static int MAX_POSITION = 360;
    private final static int MAX_POSITION_VALUE = 0x3ff;
    private final static int MIN_POSITION = 0;
    private final static int MIN_POSITION_VALUE = 0;

    public static void main(String[] args) {

        System.out.println("hello man");

        byte[] buffer = {(byte)0x02, (byte)0xF5,
                (byte)0xFD, (byte)0xFD, (byte)0x01, (byte)0x07, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x02, (byte)0xF5,
                (byte)0xFD, (byte)0xFD, (byte)0x01, (byte)0x07, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x02, (byte)0xF5,
                (byte)0xFD, (byte)0xFD, (byte)0x01, (byte)0x07, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x02, (byte)0xF5,
                (byte)0xFD, (byte)0xFD, (byte)0x01, (byte)0x07, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x02, (byte)0xF5,
                (byte)0xFD, (byte)0xFD, (byte)0x01, (byte)0x07, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x00, (byte)0x02, (byte)0xF5,
                (byte)0xFD, (byte)0xFD, (byte)0x01, (byte)0x07, (byte)0x00, (byte)0x00, (byte)0x00};

        AX12Update_ServoReadDataAnalyze analyze = new AX12Update_ServoReadDataAnalyze();

        List<AX12Update_ReceivedSerialPacket> packetList = analyze.analyzeReadData(buffer, buffer.length);

        if (packetList != null) {
            System.out.println("packetList size = " + packetList.size());
            for (AX12Update_ReceivedSerialPacket packet : packetList) {
                System.out.println("packetList packet = " + packet);

            }
        }


        List<AX12Update_ReceivedSerialPacket> packetList2 = analyze.analyzeReadData(buffer, buffer.length);

        if (packetList2 != null) {
            System.out.println("22 packetList2 size = " + packetList2.size());
            for (AX12Update_ReceivedSerialPacket packet : packetList2) {
                System.out.println("22 packetList2 packet = " + packet);

            }
        }

        List<AX12Update_ReceivedSerialPacket> packetList3 = analyze.analyzeReadData(buffer, buffer.length);

        if (packetList3 != null) {
            System.out.println("22 packetList size = " + packetList3.size());
        }
//
//        byte[] buffer = {(byte) 0xFD, (byte) 0xFD, (byte) 0x01, (byte) 0x07, (byte) 0x00,
//                (byte) 0x00, (byte) 0x01, (byte) 0x00, (byte) 0x00, (byte) 0x03, (byte) 0xF3};
//
//
//        AX12Update_ReceivedSerialPacket packet = new AX12Update_ReceivedSerialPacket(ByteBuffer.wrap(buffer));
//
//        System.out.print("packet = " + packet);
//
//        AX12Update_ReceivedSerialPacket packet2 = new AX12Update_ReceivedSerialPacket(ByteBuffer.wrap(buffer));


//        byte[] header = {(byte) 0xfd, (byte) 0xfd};
        //
//
//
//        short h = getHeaderShort(header);
//
//        System.out.print("h = " + h);

//        byte[] header = {(byte) 0xab, (byte) 0xcd};
//
//        byte[] instruction = {(byte) 0x12, (byte) 0x34};
//
//        ByteBuffer byteBuffer = ByteBuffer.allocate(1 + header.length + instruction.length);
//        byteBuffer.put(header);
//        byteBuffer.put((byte) 89);
//        byteBuffer.put(instruction);
//
////        System.out.print(UsefulUtil.toHexString(byteBuffer.array()));
//
//        byteBuffer.rewind();
//        System.out.println(String.format("position = %d", byteBuffer.position()));
//        System.out.println(String.format("int = 0x%x", byteBuffer.getShort()));
//        System.out.println(String.format("int = 0x%x", byteBuffer.get()));


//        IServoReadDataAnalyze<AX12_SerialPacket> analyze = new AX12_ServoReadDataAnalyze();
//        byte[] rBytes = new byte[]{(byte) 0xab, (byte) 0x12, (byte) 0xff, (byte) 0xff, 1, (byte) 0x3};
//        byte[] rBytes1 = new byte[]{(byte) 0x06, (byte) 0x12, (byte) 0xff, (byte) 0xff, 1, (byte) 0x2, 3};
//
//        List<AX12_SerialPacket> packetList = analyze.analyzeReadData(rBytes, rBytes.length - 1);
//        if (packetList != null) {
//            for (AX12_SerialPacket packet : packetList) {
//                System.out.println("packet = " + packet);
//            }
//        }
//        List<AX12_SerialPacket> packetList2 = analyze.analyzeReadData(rBytes1, rBytes.length);
//
//        if ( packetList2 != null) {
//            for (AX12_SerialPacket packet : packetList2) {
//                System.out.println("22 packet = " + packet);
//            }
//        }


//        float a = 1.0f * (MAX_POSITION_VALUE - MIN_POSITION_VALUE) / (MAX_POSITION - MIN_POSITION);
//        float b = (MIN_POSITION_VALUE) - a * MIN_POSITION;
//
//        System.out.println("a = " + a + " , b = " + b);
//
//        int val = toValue(0, MAX_ANGULAR_VELOCITY, MIN_ANGULAR_VELOCITY, MAX_ANGULAR_VELOCITY_VALUE, MIN_ANGULAR_VELOCITY_VALUE);
//
//        System.out.println("val = " + val);
//
//        byte[] parameters = new byte[]{(byte) 0xab, (byte) 0xcd, (byte) 0xef};
//        byte[] parameters2 = new byte[]{(byte) 0x12, (byte) 0x34, (byte) 0x56};
//
//        int p0 = getParameterValue(parameters, false);
//        int p1 = getParameterValue(parameters, true);
//
//        int p2 = getParameterValue(parameters2, false);
//        int p3 = getParameterValue(parameters2, true);
//
//        System.out.println("p0 = " + p0 + ", hex = 0x" + Integer.toHexString(p0));
//        System.out.println("p1 = " + p1 + ", hex = 0x" + Integer.toHexString(p1));
//        System.out.println("p2 = " + p2 + ", hex = 0x" + Integer.toHexString(p2));
//        System.out.println("p3 = " + p3 + ", hex = 0x" + Integer.toHexString(p3));


    }

    private static short getHeaderShort(byte[] header) {
        return (short) ((header[0] & 0xff) | ((header[1]  << 8) & 0xff00));
    }

    public static int getParameterValue(byte[] parameters, boolean isStatusFeedback) {
        if (parameters == null || parameters.length == 0) {
            return -1;
        }
        int startPos = 0;
        if (!isStatusFeedback) {
            startPos = 1;
        }

        int value = 0;
        for (int i = startPos; i < parameters.length && i < 4; i++) {
            value |= (0xff & parameters[i]) << 8 * (i - startPos);
        }
        return value;
    }

    private final static int MAX_ANGULAR_VELOCITY = 100;
    private final static int MIN_ANGULAR_VELOCITY = 0;
    private final static int MAX_ANGULAR_VELOCITY_VALUE = 0x3ff;
    private final static int MIN_ANGULAR_VELOCITY_VALUE = 0;
    private static int toValue(int _value, int max, int min, int max_v, int min_v ) {
        int value = _value;
        if (value > max) {
            value = max;
        }

        if (value < min) {
            value = min;
        }

        //y = ax +b
        float a = 1.0f * (max_v - min_v) / (max - min);
        float b = (min_v) - a * min;

        float transferValue = a * value + b;

        return (int) (0.5f + transferValue);
    }


}
